#! /usr/bin/python

import time
import m3.joint_zlift as m3z
import m3.toolbox as m3t
import m3.unit_conversion as m3u
import Numeric as nu
import m3.rt_proxy as m3p
import m3.component_factory as m3f
import m3.omnibase as m3o

proxy = m3p.M3RtProxy()
proxy.start()
proxy.make_operational_all()

#Setup Components
zlift=None
zlift_names=proxy.get_available_components('m3joint_zlift')
if len(zlift_names)!=1:
    print 'Zlift not found.'
else:
    zlift=m3z.M3JointZLift(zlift_names[0])
    proxy.subscribe_status(zlift)
    proxy.publish_command(zlift)
    proxy.publish_param(zlift) 

if zlift==None:
    proxy.stop()
    exit()
    
pwr_rt=m3t.get_joint_pwr_component_name(zlift_names[0])
pwr=m3f.create_component(pwr_rt)
proxy.publish_command(pwr)
pwr.set_motor_power_on()
proxy.make_operational_all()
proxy.step()
time.sleep(0.5)
proxy.step()

zlift_shm_names=proxy.get_available_components('m3joint_zlift_shm')
if len(zlift_shm_names) > 0:
  proxy.make_safe_operational(zlift_shm_names[0])

omnibase_shm_names=proxy.get_available_components('m3omnibase_shm')
if len(omnibase_shm_names) > 0:
  proxy.make_safe_operational(omnibase_shm_names[0])

humanoid_shm_names=proxy.get_available_components('m3humanoid_shm')
if len(humanoid_shm_names) > 0:
  proxy.make_safe_operational(humanoid_shm_names[0])

if not zlift.calibrate(proxy):
    proxy.stop()
    exit()

#Move ZLift
zlift.set_mode_theta_gc()
des=zlift.get_pos_mm()
zlift.set_slew_rate_proportion(1.0)
zlift.set_stiffness(1.0)
zlift.set_pos_mm(600.0)
proxy.step()
try:
    while True:
        print '#####'
        print des
        zlift.set_pos_mm(des)
        print '#####'
        proxy.step()
        print '-----------------------------'
        print 'u: up 2mm'
        print 'd: down 2mm'
        print 'e: enter position'
        print 'q: quit'
        print '-----------------------------'
        print
        k=m3t.get_keystroke()
        if k=='u':
            des=min(700.0,des+2)
        if k=='d':
            des=max(0,des-2)
        if k=='e':
            print 'Enter position (0-700mm)'
            des=max(0,min(700,m3t.get_float()))
        print
        print 'Des (mm)',des
        print 'Pos (mm)',zlift.get_pos_mm()
        print
        if k=='q':
            break
except (KeyboardInterrupt):
    pass
zlift.set_mode_off()
proxy.step()
proxy.stop()
